#pragma once

#include "desk.hpp"
#include "Screwdriver.hpp"
#include "HeadCamera.hpp"
#include "FinderBase.hpp"

#include "fiowin.hpp"


class FinderScrew : public FinderBase
{
public:
	Desk desk;
	Screwdriver screwdriver;

	HeadCamera headcam;

	cv::Vec6f armPos;

	cv::Vec4f deskPlane;

public:
	FinderScrew()
		: FinderBase()
	{

	}

	virtual bool ComputePath(std::vector<cv::Vec6f> &path)
	{

#if 0
		cv::Vec2f pos = desk.Recover(cv::Vec2f(pt.x,pt.y));

		path.resize(2);

		screwdriver.GetArmPos(path[0], cv::Vec3f(pos[0], pos[1], 0.1));
		screwdriver.GetArmPos(path[1], cv::Vec3f(pos[0], pos[1], 0.02));
		//screwdriver.GetArmPos(path[2], cv::Vec3f(pos[0], pos[1], 0.1));

		return true;
#endif

#if 1
		cv::Vec3f target;

		int res = headcam.RayIntersectionAtBase(target, pt.x, pt.y, armPos, deskPlane);

		if (res != 1)
		{
			std::cout << "find no target\n";
			return false;
		}

		path.resize(2);
		screwdriver.GetArmPos(path[1], target);

		target[2] += 0.1;

		screwdriver.GetArmPos(path[0], target);

		return true;
#endif
	}


	cv::Vec6f CalibDesk()
	{
		float l = 0.04115;

		cv::Vec6f armp[3] = { cv::Vec6f(0.53449,-0.13750,0.14242,1.7373,2.5698,0.0523),
			cv::Vec6f(0.54084,0.16614,0.13894,0.9370,2.8146,0.1796),
			cv::Vec6f(0.32833,0.17544,0.13565,0.5402,3.1261,-0.1165) };

		cv::Vec2f deskp[3] = { cv::Vec2f(0.187,0.007),
			cv::Vec2f(0.216,8 * l - 0.013),
			cv::Vec2f(-0.027,8 * l - 0.016) };

		cv::Vec3f tipBase[4];
		tipBase[0] = screwdriver.TipAtBase(armp[0]);
		tipBase[1] = screwdriver.TipAtBase(armp[1]);
		tipBase[2] = screwdriver.TipAtBase(armp[2]);

		tipBase[3] = (tipBase[1] - tipBase[0]).cross(tipBase[2] - tipBase[0]);

		deskPlane[0] = tipBase[3][0];
		deskPlane[1] = tipBase[3][1];
		deskPlane[2] = tipBase[3][2];
		deskPlane[3] = -tipBase[0].ddot(tipBase[3]);

		tipBase[3] /= cv::norm(tipBase[3]);
		tipBase[3] = tipBase[3] + tipBase[0];
		
		cv::Vec3f tipDesk[4];
		tipDesk[0] = cv::Vec3f(deskp[0][0], deskp[0][1], 0);
		tipDesk[1] = cv::Vec3f(deskp[1][0], deskp[1][1], 0);
		tipDesk[2] = cv::Vec3f(deskp[2][0], deskp[2][1], 0);
		
		tipDesk[3]= (tipDesk[1] - tipDesk[0]).cross(tipDesk[2] - tipDesk[0]);

		std::cout << "tipDesk[3]=" << tipDesk[3] << '\n';

		if (tipDesk[3][2] > 0)
		{
			tipDesk[3] = tipDesk[0] + cv::Vec3f(0, 0, 1);
		}
		else
		{
			tipDesk[3] = tipDesk[0] - cv::Vec3f(0, 0, 1);
		}


		cv::Vec6f desk2base = Screwdriver::ComputeTransform(tipDesk, tipBase);


		return desk2base;

	}


	void Calibrate(std::string fop="C:\\Users\\jhanbin\\Documents\\Visual Studio 2015\\Projects\\cutter\\camera\\c920\\")
	{
		cv::Vec6f desk2base = CalibDesk();

		std::vector<std::string> fnl;
		std::vector<std::string> fol;

		GetFolderContentList(fnl, fol, fop);

		std::vector<std::vector<cv::Point2f>> corners;

		std::vector<cv::Vec6f> armp;

		cv::Size ptsz(6, 9);

		cv::Size imgsz(640,480);

		for (size_t i = 0;i < fnl.size();i++)
		{
			if (CheckExt(fnl[i], "png"))
			{
				corners.push_back(std::vector<cv::Point2f>());
				if (FindCorners(corners.back(), cv::imread(fop+fnl[i]), ptsz))
				{
					std::string txt = fnl[i].substr(0, fnl[i].size() - 3) + "txt";
					float tmp[6];
					LoadPos(tmp, 6, fop + txt);
					armp.push_back(cv::Vec6f(tmp));
				}
				else
				{
					corners.pop_back();
				}
				//cv::waitKey();
			}
		}

		headcam.Calibrate(armp, desk2base, corners, imgsz, ptsz);

	}
};